#include "kernel/initializer.hpp"
#include "yocto/code/rand.hpp"
#include "yocto/math/kernel/algebra.hpp"

void Initializer:: compile()
{
	try {
		
		std::cerr << "---------------- Compiling Initializer ----------------" << std::endl;
		
		(size_t&)Nc = I.size();
		(size_t&)M  = library.size();
		if( Nc > M )
			throw exception("*** Initializer: too many constraints!" );
		(size_t&)N  = M-Nc;
		
		C.make(Nc,M);
		Q.make(M,N);
		X0.make(M,0.0);
		lss.ensure(M);
		
		//----------------------------------------------------------------------
		//-- compute constraint matrix
		//----------------------------------------------------------------------
		for( size_t i=1; i <= Nc; ++i )
		{
			const ChemIni &ci = I[i];
			for( size_t k=ci.terms_.size(); k>0;--k)
			{
				const ChemIni::Term &t = ci.terms_[k];
				const Species       &s = library[ t.name ];
				const size_t         j = s.idx;
				C[i][j] = t.weight;
			}
		}
		//std::cerr << "C=" << C << std::endl;
		
		//----------------------------------------------------------------------
		//-- compute its pseudo inverse P = C' * inv(C*C')
		//----------------------------------------------------------------------
		matrix<Real> P(M,Nc);
		matrix<Real> C2(Nc,Nc);
		matrix<Real> iC2(Nc,Nc);
		for( size_t i=1; i <= Nc; ++i )
		{
			for(size_t j=i; j <= Nc; ++j )
			{
				Real sum = 0;
				for( size_t k=1; k<=M; ++k )
				{
					sum += C[i][k] * C[j][k];
				}
				C2[i][j] = C2[j][i] = sum;
			}
		}
		if( !lss.LU(C2) )
			throw exception("*** Initializer: singular constraints");
		iC2.ld1();
		lss(C2,iC2);
		//std::cerr << "C2=" << C2 << std::endl;
		//std::cerr << "iC2=" << iC2 << std::endl;
		algebra<Real>::mul_ltrn(P,C,iC2);
		//std::cerr << "P=" << P << std::endl;
		
		matrix<Real> Qa(M,M);
		matrix<Real> Qb(M,M);
		//----------------------------------------------------------------------
		//-- build local matrix: complete C in an inversible matrix
		//----------------------------------------------------------------------
		do {
			for( size_t i=1; i <= Nc; ++i )
			{
				for( size_t j=1; j <= M; ++j )
				{
					Qa[i][j] = C[i][j];
				}
				
			}
			for( size_t i=Nc+1; i <= M; ++i )
			{
				for( size_t j=1; j <= M; ++j )
				{
					Qa[i][j] = 0.5 - alea<Real>();
				}
				
			}
			Qb.assign(Qa);
		} while ( !lss.LU(Qa) );
		//std::cerr << "Qb=" << Qb << std::endl;
		
		algebra<Real>::gram_schmidt(Qa,Qb,true);
		//std::cerr << "Qa=" << Qa << std::endl;
		
		for( size_t i=1; i <=M; ++i )
		{
			for( size_t j=1; j <= N; ++j )
			{
				Q[i][j] = Qa[j+Nc][i];
			}
		}
		
		//std::cerr << "Q=" << Q << std::endl;
		
		//----------------------------------------------------------------------
		//-- compute vector of constants
		//----------------------------------------------------------------------
		vector<Real> V0(Nc,0);
		for( size_t i=1; i <= Nc; ++i )
		{
			V0[i] = I[i].value;
		}
		//std::cerr << "V0=" << V0 << std::endl;
		
		algebra<Real>::mul(X0,P,V0);
		std::cerr << "X0=" << X0 << std::endl;
		
		//----------------------------------------------------------------------
		//-- Check no negative immutable concentration
		//----------------------------------------------------------------------
		for( size_t i=1; i <= M; ++i )
		{
			if( X0[i] < 0 )
			{
				const array<Real> &Qi = Q[i];
				Real sum = 0;
				for( size_t j=1; j <= N; ++j )
					sum += Fabs(Qi[j]);
				if( sum <= 0)
					throw exception("*** Initializer: negative immutable [%s]!!", library(i).name.c_str());
			}
		}
		
		std::cerr << "---------------- Compiled  Initializer ----------------" << std::endl << std::endl;
		
	}
	catch(...) {
		(size_t&)Nc = 0;
		(size_t&)M  = 0;
		(size_t&)N  = 0;
		throw;
	}
}

static inline 
void __build( array<Real> &X, const array<Real> &X0, const matrix<Real> &Q, const array<Real> &Y )
{
	algebra<Real>::mul(X,Q,Y);
	for( size_t i=X.size();i>0;--i) 
		X[i] += X0[i];
}

void Initializer:: setup( ChemSys &chemsys, Real t)
{
	
	assert(Nc>0);
	assert(M>0);
	assert(N>0);
	
	assert(chemsys.N>0);
	assert(chemsys.M>0);
	assert(chemsys.ftol>0);
	std::cerr << "---------------- Initializer Setup ----------------" << std::endl;
	
	//==========================================================================
	// Sanity check
	//==========================================================================
	if( & chemsys.library != &library )
		throw exception("*** Initializer: mismatch ChemSys library");
	if( chemsys.M != this->M )
		throw exception("*** Initializer: mismatch ChemSys size");
	if( chemsys.N != N )
		throw exception("*** Initializer: mismatch ChemSys dimension");
	
	const Real   ftol = chemsys.ftol;
	matrix<Real> PhiQ(N,N);
	array<Real>  &X     = chemsys.X;
	array<Real>  &Gamma = chemsys.Gamma; 
	matrix<Real> &Phi   = chemsys.Phi;
	
	
	//==========================================================================
	// Initialize Algorithm
	//==========================================================================
	vector<Real>  Y(N,0.0);
	for( size_t i=1; i <= M; ++i ) X[i] = C0;
	algebra<Real>::mul_trn(Y,Q,X);
	__build(X, X0, Q, Y);
	vector<Real>  dY(N,0.0);
	vector<Real>  dX(M,0.0);
	
	
	
	//==========================================================================
	// Newton
	//==========================================================================
	bool converged = false;
	do
	{
		chemsys.computeGammaAndPhi(0, false);
		algebra<Real>::mul(PhiQ,Phi,Q);
		for( size_t i=1; i <= N; ++i ) dY[i] = -Gamma[i];
		//std::cerr << "PhiQ=" << PhiQ << std::endl;
		
		if( !lss.LU(PhiQ) )
			throw exception("*** Initializer: Singular SetUp!");
		lss(PhiQ,dY);
		//std::cerr << "dY=" << dY << std::endl;
		
		//----------------------------------------------------------------------
		// update and test convergence
		//----------------------------------------------------------------------
		converged = true;
		for( size_t i=1; i <= N; ++i )
		{
			const Real dy   = dY[i];
			const Real yold = Y[i];
			const Real ynew = yold + dy;
			Y[i] = ynew;
			if( Fabs(dy) > ftol * ( Fabs(yold) + Fabs(ynew) ) )
			{
				converged = false;
			}
		}
		//std::cerr << "Y=" << Y << std::endl;
		__build(X, X0, Q, Y);
		
	}
	while( !converged );
	//std::cerr << "X=" << X << std::endl;

	//==========================================================================
	// Numeric correction: avoid negative concentrations
	//==========================================================================
	vector<Real> X1(M,0.0);
	const Real   min_conc = 0.1 * MinimalConcentration;
	for(;;)
	{
		bool all_positive = true;
		for( size_t i=1; i <= M; ++i )
		{
			const Real x = X[i];
			if( x > 0 ) 
				X1[i] = x;
			else 
			{
				if( Fabs(x) >= min_conc ) 
					all_positive = false;
				X1[i] = 0;
			}
		}
		for( size_t i=1; i <= M; ++i )
		{
			X1[i] -= X[i];
		}
		algebra<Real>::mul_trn(Y,Q,X1);
		algebra<Real>::mul(X1,Q,Y);
		for( size_t i=1; i <= M; ++i )
		{
			X[i] += X1[i];
		}
		if( all_positive )
			break;
	}
	
	//==========================================================================
	// Final Cleanup
	//==========================================================================
	for( size_t i=1; i <= M; ++i )
	{
		if( Fabs( X[i] ) <= MinimalConcentration )
			X[i] = 0;
	}
	std::cerr << "X=" << X << std::endl;
	
	std::cerr << "---------------- Initializer Setup ----------------" << std::endl << std::endl;
}

